//------------------------------------------------------------------------------
//
//  Hemisson Drive Service
//
//  Written Dec-2007 by Trevor Taylor, Software Technology, Australia
//
//  Implemented DriveDistance and RotateDegrees, but they are a fudge
//  using timers
//
//  NOTE: This service works fine with most other applications like the
//  Dashboard, TeleOperation, etc. However, it does not faithfully update
//  the WheelSpeed property in the Wheel states. It only updates the
//  CurrentPower in the MotorState.
//
//------------------------------------------------------------------------------

using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using Microsoft.Dss.Services.SubscriptionManager;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using W3C.Soap;
using drive = Microsoft.Robotics.Services.Drive.Proxy;
//using dssphttp = Microsoft.Dss.Core.DsspHttp;
using xml = System.Xml;

// Access other parts of the solution
using ProMRDS.Robotics.Hemisson;

// Add references for motor and physical model
using motor = Microsoft.Robotics.Services.Motor.Proxy;
using model = Microsoft.Robotics.PhysicalModel.Proxy;

// The Hemisson is a Generic Brick
using brick = ProMRDS.Robotics.GenericBrick.Proxy;

// For HttpGet
using Microsoft.Dss.Core.DsspHttp;
// For HttpStatusCode
using System.Net;

// Specifically for HttpPost handling
//using System.Collections.ObjectModel;
using System.Collections.Specialized;
using Microsoft.Dss.Core.DsspHttpUtilities;


namespace ProMRDS.Robotics.Hemisson.Drive
{
    /// <summary>
    /// Provides access to a differential drive (that coordinates two motors that function together).
    /// </summary>
    [Contract(Contract.Identifier)]
    [AlternateContract("http://schemas.microsoft.com/robotics/2006/05/drive.html")]
    [DisplayName("Hemisson Generic Differential Drive")]
    [Description("Provides access to the Hemisson Drive\n(Uses the Generic Differential Drive contract)\n(Partners with Hemisson 'brick')")]
    public class HemissonDriveService : DsspServiceBase
    {
        /// <summary>
        /// Hemisson Drive Main Port
        /// </summary>
        [ServicePort("/hemissondrive", AllowMultipleInstances=false)]
        private drive.DriveOperations _mainPort = new drive.DriveOperations();
        
        /// <summary>
        /// Hemisson Drive State
        /// </summary>
        [InitialStatePartner(Optional=true, ServiceUri="Hemisson.Drive.Config.xml")]
        private drive.DriveDifferentialTwoWheelState _state = new drive.DriveDifferentialTwoWheelState();

        // Transform file for displaying state
        [EmbeddedResource("ProMRDS.Robotics.Hemisson.Resources.HemissonDrive.xslt")]
        string _transform = null;

        [EmbeddedResource("ProMRDS.Robotics.Hemisson.Resources.Fault.xslt")]
        string _faultTransform = null;

        // Required for Http Post handling
        // NOTE: Do NOT create a new port because it gives you the mistaken idea
        // that you can send messages to it when in fact there is no service to
        // respond to the messages! You must create the new service during the
        // initialization process -- see Start(). Leave it null and your code
        // will crash if you forget to create the service.
        //DsspHttpUtilitiesPort _httpUtilities = new DsspHttpUtilitiesPort();
        DsspHttpUtilitiesPort _httpUtilities;

        /// <summary>
        /// Microsoft.Robotics.Services.Drive.HemissonDrive Partner
        /// </summary>
        [Partner("SubMgr", Contract = "http://schemas.microsoft.com/xw/2005/01/subscriptionmanager.html", CreationPolicy = PartnerCreationPolicy.CreateAlways, Optional = false)]
        private SubscriptionManagerPort _subMgrPort = new Microsoft.Dss.Services.SubscriptionManager.SubscriptionManagerPort();

        // Partner with the Hemisson Brick service
        [Partner("Hemisson", Contract = brick.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate, Optional = false)]
        private brick.GenericBrickOperations _brickPort = new brick.GenericBrickOperations();

        // Used for driving the motors. Always execute the newest pending drive request.
        private Port<drive.SetDrivePower> _internalDrivePowerPort = new Port<drive.SetDrivePower>();

        // Used for stopping the motors after a timed motion
        private Port<StopMotion> _internalStopPort = new Port<StopMotion>();
        // Flag to indicate that a Stop is pending for a timed motion
        bool _ackPending = false;
        // Counter for ACKs (Every ACK has a unique ID)
        int _ackCounter = 0;
        // Current response port (Used to cancel outstanding requests)
        PortSet<DefaultUpdateResponseType, Fault> _ackPort = null;

        // Used for handling HttpPost Success messages
        private Port<HttpPostSuccess> _httpPostPort = new Port<HttpPostSuccess>();

        /// <summary>
        /// Default Service Constructor
        /// </summary>
        public HemissonDriveService(DsspServiceCreationPort creationPort) : 
                base(creationPort)
        {
        }
        
        /// <summary>
        /// Service Start
        /// </summary>
        protected override void Start()
        {
            InitializeState();

            // Needed for HttpPost
            _httpUtilities = DsspHttpUtilitiesService.Create(Environment);

            // Now add ourselves to the directory
            base.Start();

            // Wait one time for an InternalDrivePower command
            Activate(Arbiter.ReceiveWithIterator<drive.SetDrivePower>(false, _internalDrivePowerPort, InternalDrivePowerHandler));

            // Set up the Stop Motion handler which must be in the Exclusive group
            // along with the other drive requests
            MainPortInterleave.CombineWith(
                new Interleave(
                    new ExclusiveReceiverGroup(
                        Arbiter.ReceiveWithIterator<StopMotion>(true, _internalStopPort, StopMotionHandler)
                    ),
                    new ConcurrentReceiverGroup(
                        Arbiter.ReceiveWithIterator<HttpPostSuccess>(true, _httpPostPort, HttpPostSuccessHandler)
                    )
                )
            );
        }

        #region Initialization

        /// <summary>
        /// Initialize the drive state
        /// </summary>
        private void InitializeState()
        {
            // Create a new state if there is none
            if (_state == null)
                _state = new drive.DriveDifferentialTwoWheelState();

            // Sanity check (or reset) all of the state properties

            _state.DriveState = drive.DriveState.Stopped;
            _state.IsEnabled = false;

            // The distance between wheels is not exact because the
            // actual contact points of the wheels with the ground
            // might not be in the middle of the wheels.
            // The distance is about 10.25cm from edge to edge.
            if (_state.DistanceBetweenWheels == 0.0)
                _state.DistanceBetweenWheels = 0.10;

            if (_state.LeftWheel == null)
                _state.LeftWheel = new motor.WheeledMotorState();
            if (_state.RightWheel == null)
                _state.RightWheel = new motor.WheeledMotorState();

            _state.LeftWheel.WheelSpeed = 0.0;
            _state.RightWheel.WheelSpeed = 0.0;

            if (_state.LeftWheel.GearRatio == 0.0)
                _state.LeftWheel.GearRatio = 1.0;
            if (_state.RightWheel.GearRatio == 0.0)
                _state.RightWheel.GearRatio = 1.0;

            // Wheel radius on the Hemisson is tiny!
            // Only about 1.5cm, but this cannot be measured very
            // accurately and there will also be effects as the
            // wheels sink into carpet, etc.
            if (_state.LeftWheel.Radius == 0.0)
                _state.LeftWheel.Radius = 0.015;
            if (_state.RightWheel.Radius == 0.0)
                _state.RightWheel.Radius = 0.015;

            model.Pose p = new model.Pose();
            p.Position = new model.Vector3(-0.05f, 0.015f, 0);
            p.Orientation = new model.Quaternion(-1, 0, 0, 0);
            if (_state.LeftWheel.MotorState == null)
                _state.LeftWheel.MotorState = new motor.MotorState("Left Motor", 1, 9.0, false, p);
            if (_state.LeftWheel.MotorState.HardwareIdentifier == 0)
                _state.LeftWheel.MotorState.HardwareIdentifier = 1;
            if (string.IsNullOrEmpty(_state.LeftWheel.MotorState.Name))
                _state.LeftWheel.MotorState.Name = "Left Motor";
            if (_state.LeftWheel.MotorState.PowerScalingFactor == 0.0)
                _state.LeftWheel.MotorState.PowerScalingFactor = 9.0;

            p.Position = new model.Vector3(0.05f, 0.015f, 0);
            p.Orientation = new model.Quaternion(1, 0, 0, 0);
            if (_state.RightWheel.MotorState == null)
                _state.RightWheel.MotorState = new motor.MotorState("Right Motor", 2, 9.0, true, p);
            if (_state.RightWheel.MotorState.HardwareIdentifier == 0)
                _state.RightWheel.MotorState.HardwareIdentifier = 2;
            if (string.IsNullOrEmpty(_state.RightWheel.MotorState.Name))
                _state.RightWheel.MotorState.Name = "Right Motor";
            if (_state.RightWheel.MotorState.PowerScalingFactor == 0.0)
                _state.RightWheel.MotorState.PowerScalingFactor = 9.0;

            SaveState(_state);
        }

        #endregion

        /// <summary>
        /// Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetHandler(drive.Get get)
        {
            get.ResponsePort.Post(_state);
            yield break;
        }
        
        /// <summary>
        /// HttpGet Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> HttpGetHandler(HttpGet get)
        {
            get.ResponsePort.Post(new HttpResponseType(
                HttpStatusCode.OK,
                _state,
                _transform)
            );
            yield break;
        }
        
        /// <summary>
        /// HttpPost Handler
        /// </summary>
        /// <param name="submit"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> HttpPostHandler(HttpPost submit)
        {
            // Used for submitting success messages
            HttpPostSuccess success = new HttpPostSuccess();
            // Accumulates error messages
            string ErrorMessage = string.Empty;
            Fault fault = null;
            NameValueCollection parameters = new NameValueCollection();
            // Use helper to read form data
            // TT Jul-2008 - Changed in July CTP
            //ReadFormData readForm = new ReadFormData(submit.Body.Context);
            ReadFormData readForm = new ReadFormData(submit);
            _httpUtilities.Post(readForm);

            // Wait for result
            yield return Arbiter.Choice(
                readForm.ResultPort,
                delegate(NameValueCollection col)
                {
                    parameters = col;
                },
                delegate(Exception e)
                {
                    fault = Fault.FromException(e);
                    LogError(null, "Error processing form data", fault);
                    ErrorMessage += e.Message;
                }
            );

            if (fault != null)
            {
//                submit.ResponsePort.Post(fault);
                HttpPostFailure(submit, fault);
                yield break;
            }

            // For debugging only ...
            /*
            Console.WriteLine("Form Parameters:");
            for (int i = 0; i < parameters.Count; i++)
            {
                Console.WriteLine(parameters[i]);
            }
             */

            double LeftPower = _state.LeftWheel.MotorState.CurrentPower;
            double RightPower = _state.RightWheel.MotorState.CurrentPower;
            bool validValue = false;

            if (parameters["Stop"] != null)
            {
                drive.AllStop req = new drive.AllStop();
                _mainPort.Post(req);
                success.Body.WaitPort = req.ResponsePort;
            }
            else if (parameters["EnableDrive"] != null)
            {
                drive.EnableDrive req = new drive.EnableDrive();
                req.Body.Enable = true;
                _mainPort.Post(req);
                success.Body.WaitPort = req.ResponsePort;
            }
            else if (parameters["DisableDrive"] != null)
            {
                drive.EnableDrive req = new drive.EnableDrive();
                req.Body.Enable = false;
                _mainPort.Post(req);
                success.Body.WaitPort = req.ResponsePort;
            }
            else if (parameters["SetPower"] != null)
            {
                if (_state.IsEnabled)
                {
                    if (!string.IsNullOrEmpty(parameters["LeftPower"]))
                    {
                        try
                        {
                            LeftPower = double.Parse(parameters["LeftPower"]);
                            if (LeftPower >= -1.0 && LeftPower <= 1.0)
                                validValue = true;
                            else
                                ErrorMessage += "Invalid Left Power setting";
                        }
                        catch (Exception e)
                        {
                            string msg = "Could not parse Left Power: " + e.Message;
                            LogError(msg);
                            ErrorMessage += msg;
                        }
                    }

                    validValue = false;
                    if (!string.IsNullOrEmpty(parameters["RightPower"]))
                    {
                        try
                        {
                            RightPower = double.Parse(parameters["RightPower"]);
                            if (RightPower >= -1.0 && RightPower <= 1.0)
                                validValue = true;
                            else
                                ErrorMessage += "Invalid Right Power setting";
                        }
                        catch (Exception e)
                        {
                            string msg = "Could not parse Right Power: " + e.Message;
                            LogError(msg);
                            ErrorMessage += msg;
                        }
                    }

                    // Finally, process the result
                    if (ErrorMessage == string.Empty)
                    {
                        drive.SetDrivePower update = new drive.SetDrivePower();

                        if (LeftPower == 0 && RightPower == 0)
                            _state.DriveState = drive.DriveState.Stopped;
                        else
                            _state.DriveState = drive.DriveState.DrivePower;

                        // Pass the request to the internal handler
                        update.Body.LeftWheelPower = LeftPower;
                        update.Body.RightWheelPower = RightPower;
                        _internalDrivePowerPort.Post(update);
                        success.Body.WaitPort = update.ResponsePort;
                    }
                }
                else
                    ErrorMessage = "Drive is not enabled";
            }

            if (ErrorMessage == string.Empty)
            {
                // Post a message to finally issue the HTTP response
                // IMPORTANT NOTE:
                // If we simply redisplay the state now, it will be WRONG!
                // It is quite likely that we have issued a request to an
                // Exclusive handler in the code above. If we wait for a
                // response, then we create a Deadlock!
                // The solution is to post a message, which gives the other
                // handler a chance to run, then wait for the response, and
                // finally send the HTTP page back to the user. Got that?
                success.Body.Submit = submit;
                // This is the ONLY place where these messages are sent
                // Do NOT wait for a response!
                _httpPostPort.Post(success);
//                HttpPostSuccess(submit);
            }
            else
                HttpPostFailure(submit, ErrorMessage);

            yield break;
        }

        /// <summary>
        /// Send Http Post Success Handler
        /// </summary>
        /// Processes successful HttpPost messages by first waiting for the
        /// requested task to be performed, then sending back the web page
        /// NOTE: This is a complicated process, but it is necessary to avoid
        /// creating a Deadlock!
        private IEnumerator<ITask> HttpPostSuccessHandler(HttpPostSuccess success)
        {
            Fault fail = null;

            // Wait for the request to be processed
            // Otherwise, the settings displayed on the web page are the
            // OLD state settings, not the new ones!
            if (success.Body.WaitPort != null)
            {
                yield return Arbiter.Choice(
                    success.Body.WaitPort,
                    delegate(DefaultUpdateResponseType resp) { },
                    delegate(Fault f) { fail = f; }
                );
            }

            // Make sure that the requested task completed successfully
            if (fail == null)
            {
                // Post a HTTP response (finally!)
                HttpResponseType rsp =
                    new HttpResponseType(HttpStatusCode.OK, _state, _transform);
                success.Body.Submit.ResponsePort.Post(rsp);
            }
            else
            {
                // There is still a chance for this to fail!
                HttpPostFailure(success.Body.Submit, fail);
            }
        }

        /*
        /// <summary>
        /// Send Http Post Success Response
        /// </summary>
        private void HttpPostSuccess(HttpPost httpPost)
        {
            // Post a response
            HttpResponseType rsp =
                new HttpResponseType(HttpStatusCode.OK, _state, _transform);
            httpPost.ResponsePort.Post(rsp);
        }
        */

        /// <summary>
        /// Send Http Post Failure Response
        /// </summary>
        private void HttpPostFailure(HttpPost httpPost, string failureReason)
        {
            // Create a new Fault based on the error message
            Fault fault = Fault.FromCodeSubcodeReason(FaultCodes.Receiver, DsspFaultCodes.OperationFailed, failureReason);

            // Post it back but use the Fault Transform
            httpPost.ResponsePort.Post(new HttpResponseType(
                HttpStatusCode.OK,
                fault,
                _faultTransform
                )
            );
        }

        /// <summary>
        /// Send Http Post Failure Response
        /// </summary>
        private void HttpPostFailure(HttpPost httpPost, Fault f)
        {
            // Post back the Fault but use the Fault Transform
            httpPost.ResponsePort.Post(new HttpResponseType(
                HttpStatusCode.OK,
                f,
                _faultTransform
                )
            );
        }
        
        /// <summary>
        /// ReliableSubscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> ReliableSubscribeHandler(drive.ReliableSubscribe subscribe)
        {
            base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort);
            yield break;
        }
        
        /// <summary>
        /// Subscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> SubscribeHandler(drive.Subscribe subscribe)
        {
            base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort);
            yield break;
        }
        
        /// <summary>
        /// Update Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> UpdateHandler(drive.Update update)
        {
            // TODO: Implement Update operations here.
            throw new NotImplementedException("Update is not implemented");
        }
        
        /// <summary>
        /// EnableDrive Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> EnableDriveHandler(drive.EnableDrive update)
        {
            // Stop now if drive is to be disabled
            if (!update.Body.Enable)
            {
                ClearPendingStop();
                // Can't post a normal SetDrivePower message because the drive will be
                // disabled already! Internal drive power is not affected by Enable.
                _internalDrivePowerPort.Post(new drive.SetDrivePower(new drive.SetDrivePowerRequest(0, 0)));
                // However, now the drive power settings will not be updated until
                // AFTER this request completes. So cheat!
                _state.LeftWheel.MotorState.CurrentPower = 0;
                _state.RightWheel.MotorState.CurrentPower = 0;
                _state.DriveState = drive.DriveState.Stopped;
            }

            _state.IsEnabled = update.Body.Enable;
            _state.TimeStamp = DateTime.Now;
            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);

            yield break;
        }
        
        /// <summary>
        /// SetDrivePower Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> SetDrivePowerHandler(drive.SetDrivePower update)
        {
            // Return a Fault if the drive is not enabled
            if (!_state.IsEnabled)
            {
                update.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Drive is not enabled")));
                yield break;
            }

            // All motion requests must do this first!
            ClearPendingStop();

            /*
            Port<drive.SetDrivePower> p = _mainPort;
            if (p.ItemCount > 10)
                Console.WriteLine("Backlog of " + p.ItemCount);
            */

            if (update.Body.LeftWheelPower == 0 && update.Body.RightWheelPower == 0)
                _state.DriveState = drive.DriveState.Stopped;
            else
                _state.DriveState = drive.DriveState.DrivePower;

            // Pass the request to the internal handler
            _internalDrivePowerPort.Post(update);
        }
        
        /// <summary>
        /// SetDriveSpeed Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> SetDriveSpeedHandler(drive.SetDriveSpeed update)
        {
            if (!_state.IsEnabled)
            {
                update.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Drive is not enabled")));
                yield break;
            }

            // All motion requests must do this first!
            ClearPendingStop();

            // TODO: Convert meters/second to power based on radius of wheels.
            // There has to be another "fudge factor" that converts m/s to a power setting
            // Many of the available services do not implement speed because it is too hard!

            // Force this via the internal port so that it gets queued
            // with the rest of the drive requests
            drive.SetDrivePower power = new drive.SetDrivePower();
            // Just copy speed across to power
            // Obviously this is NOT correct, but whatcha gonna do?
            power.Body.LeftWheelPower = update.Body.LeftWheelSpeed;
            power.Body.RightWheelPower = update.Body.RightWheelSpeed;
            // Copy the response port too so that the sender will get a reply
            power.ResponsePort = update.ResponsePort;

            if (power.Body.LeftWheelPower == 0 && power.Body.RightWheelPower == 0)
                _state.DriveState = drive.DriveState.Stopped;
            else
                _state.DriveState = drive.DriveState.DriveSpeed;

            // Use the internal handler for throttling
            _internalDrivePowerPort.Post(power);
        }

        #region Timed Moves

        // Experimentally determined time factors to travel a specified distance
        // Then adjusted slightly after retesting
        private static int[] TimeFactors =
            {
                50000,      // 0
                50000,      // 1
                50000,      // 2
                39200,      // 3
                22100,      // 4
                14500,      // 5
                14500,      // 6
                11100,      // 7
                9200,       // 8
                7740,       // 9
                6500        // 10
            };

        /// <summary>
        /// CalculateTimeToMove
        /// </summary>
        /// <param name="distance">Distance to travel</param>
        /// <param name="power">Drive power to use</param>
        /// <returns>Number of milliseconds</returns>
        public int CalculateTimeToMove(double distance, double power)
        {
            // Drive distance is in meters, but we have to convert it to a
            // time delay in milliseconds. The "magic" numbers here were found
            // by trial and error using a 30cm drive distance. It will change
            // as the batteries run down and due to a variety of other factors.
            int pwr = (int)(Math.Abs(power) * 10 + 0.5);
            double delay = Math.Abs(distance) * TimeFactors[pwr];
            return (int)(delay);
        }


        /// <summary>
        /// RotateDegrees Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> RotateDegreesHandler(drive.RotateDegrees update)
        {
            // This handler is basically the same as DriveDistance except for the
            // calculation of the distance. It could use DriveDistance, but that
            // is an unnecessary overhead.
            // See DriveDistance for more comments.

            // Time delay for the motion
            int delay;
            // Speeds for wheels to move at
            double leftSpeed;
            double rightSpeed;
            // Distance wheels will travel during rotation
            double arcDistance;

            if (!_state.IsEnabled)
            {
                update.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Drive is not enabled")));
                yield break;
            }

            // All motion requests must do this first!
            ClearPendingStop();

            // Check for zero degrees first because we don't want the wheels to jerk
            // if the robot is supposed to go nowhere!
            if (update.Body.Degrees == 0.0)
            {
                update.ResponsePort.Post(new DefaultUpdateResponseType());
                yield break;
            }
            else if (update.Body.Degrees < 0.0)
            {
                // Note that the speeds are opposite to make the robot
                // rotate on the spot
                leftSpeed = update.Body.Power;
                rightSpeed = -update.Body.Power;
            }
            else
            {
                leftSpeed = -update.Body.Power;
                rightSpeed = update.Body.Power;
            }

            // Calculate the distance that the wheels must travel for the given
            // turn angle. They will move around the circumference of a circle
            // with a diameter equal to the wheel base. Each 360 degrees of rotation
            // is one full circumference.
            arcDistance = Math.Abs(update.Body.Degrees) * _state.DistanceBetweenWheels * Math.PI / 360;

            // Calculate the delay time
            delay = CalculateTimeToMove(arcDistance, update.Body.Power);

            // Start the motors running (in opposite directions)
            drive.SetDrivePower power = new drive.SetDrivePower();
            // Set the power
            power.Body.LeftWheelPower = leftSpeed;
            power.Body.RightWheelPower = rightSpeed;
            // Do NOT respond to this message (Happens later on Stop)
            power.ResponsePort = null;

            // Use the internal handler for throttling
            _internalDrivePowerPort.Post(power);
            _state.DriveState = drive.DriveState.RotateDegrees;

            // Set the timer and forget about it
            SetStopTimer(delay, update.ResponsePort);
        }
        
        /// <summary>
        /// DriveDistance Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> DriveDistanceHandler(drive.DriveDistance update)
        {
            // Drive Distance can be used to move a specified distance.
            //
            // Use a timer -- start the motors, wait, stop the motors.
            //
            // The problem is that using timers on Windows is not
            // reliable. On top of that, there are transmission delays
            // when sending commands, to it is not possible to make very
            // accurate moves this way.
            //

            // Time delay for the motion
            int delay;
            // Speed to move at
            // NOTE: "speed" is really the power because robot does not implement both.
            // Power values should be between 0 and 1.
            // The speed should be calculated based on the wheel radius and power,
            // but this is not implemented.
            double speed = Math.Abs(update.Body.Power);
            // Distance to travel
            // NOTE: Microsoft says that the Distance should always be POSITIVE and
            // that you use a negative Power to move backwards. I believe that a
            // convention of negative distances should be acceptable. Therefore
            // the code therefore checks both possibilities. This leads to the
            // strange case of a negative distance AND power, this is forwards!
            double distance = update.Body.Distance * Math.Sign(update.Body.Power);

            if (!_state.IsEnabled)
            {
                update.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Drive is not enabled")));
                yield break;
            }

            // All motion requests must do this first!
            ClearPendingStop();

            // Check for zero distance first because we don't want the wheels to jerk
            // if the robot is supposed to go nowhere!
            if (distance == 0.0)
            {
                update.ResponsePort.Post(new DefaultUpdateResponseType());
                yield break;
            }
            else if (distance < 0.0)
            {
                // Speed needs to be negative if distance is negative
                speed = -speed;
            }
            else
            {
                // Speed remains the same (positive)
            }

            // Calculate the delay time
            delay = CalculateTimeToMove(distance, speed);

            // Start the motors running!
            drive.SetDrivePower power = new drive.SetDrivePower();
            // Set the power
            power.Body.LeftWheelPower = speed;
            power.Body.RightWheelPower = speed;
            // Do NOT respond to this message (Happens later on Stop)
            power.ResponsePort = null;

            // Use the internal handler for throttling
            _internalDrivePowerPort.Post(power);
            _state.DriveState = drive.DriveState.DriveDistance;

            // Set the timer and forget about it
            SetStopTimer(delay, update.ResponsePort);
        }

        #endregion

        /// <summary>
        /// AllStop Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> AllStopHandler(drive.AllStop update)
        {
            // All motion requests must do this first!
            ClearPendingStop();

            // Kill all outstanding requests!
            // Because this is Exclusive, there cannot be another drive operation
            // in progress so deleting them all should not be a problem.

            // Take a snapshot of the number of pending commands at the time
            // we entered this routine.
            // This will prevent a livelock which can occur if we try to
            // process the queue until it is empty, but the inbound queue
            // is growing at the same rate as we are pulling off the queue.
            int pendingCommands = _internalDrivePowerPort.ItemCount;

            // Send success responses to all the outstanding requests
            // Should we send a Fault because the operation was cancelled???
            drive.SetDrivePower power;
            while (pendingCommands > 0)
            {
                if (_internalDrivePowerPort.Test(out power))
                {
                    // Timed motion requests do not have a response initially,
                    // that comes later when the motors are stopped
                    if (power.ResponsePort != null)
                        power.ResponsePort.Post(DefaultUpdateResponseType.Instance);
                }
                pendingCommands--;
            }

            _state.DriveState = drive.DriveState.Stopped;

            // Explicitly send a stop command to the brick
            yield return Arbiter.Choice(_brickPort.SetDrivePower(new drive.SetDrivePowerRequest(0.0, 0.0)),
                delegate(DefaultUpdateResponseType ok)
                {
                    _state.LeftWheel.MotorState.CurrentPower = 0;
                    _state.RightWheel.MotorState.CurrentPower = 0;
                    update.ResponsePort.Post(ok);
                },
                delegate(Fault fault)
                {
                    update.ResponsePort.Post(fault);
                }
            );
        }

        #region Internal Handlers

        // Issue a fault if a pending Stop was terminated and
        // clear the flag
        void ClearPendingStop()
        {
            if (_ackPending)
            {
                _ackPending = false;
                Fault f;
                f = Fault.FromException(new Exception("Stop after timed motion aborted"));
                _ackPort.Post(f);
                _ackPort = null;
            }
        }

        // Set a timer to post a Stop later
        void SetStopTimer(int delay, PortSet<DefaultUpdateResponseType, Fault> p)
        {
            int num;

            // Classic case of "should never happen"
            ClearPendingStop();

            // Increment the ACK counter to make it unique and
            // keep a copy for later
            _ackCounter++;
            num = _ackCounter;
            // Remember the current response port in case it has
            // to be cancelled
            _ackPort = p;
            // Enable ACK
            _ackPending = true;

            // Setup a delegate for the specified time period
            // It will post a StopMotion message when it fires
            Activate(Arbiter.Receive(
                false,
                TimeoutPort(delay),
                delegate(DateTime timeout)
                {
                    // Send ourselves a Stop request using the ACK number
                    _internalStopPort.PostUnknownType(new StopMotion(num));
                }
            ));
        }

        /// <summary>
        /// Internal Stop Motion Handler
        /// Called in response to a Stop message when a timed motion finishes
        /// </summary>
        /// <param name="stop"></param>
        /// <returns></returns>
        public virtual IEnumerator<ITask> StopMotionHandler(StopMotion stop)
        {
            // Somebody beat us to it!
            if (!_ackPending)
                yield break;

            // Another request has arrived and overridden this one
            // so there is still one pending, but it is not us
            if (stop.Body.AckNumber != _ackCounter)
                yield break;

            // Now stop the robot and post a response back to say the move is complete
            drive.SetDrivePower power = new drive.SetDrivePower();
            // Set the power
            power.Body.LeftWheelPower = 0;
            power.Body.RightWheelPower = 0;
            // Set the response port
            power.ResponsePort = _ackPort;

            // Use the internal handler for throttling
            _internalDrivePowerPort.Post(power);
            _state.DriveState = drive.DriveState.Stopped;

            // Clear the ACK
            _ackPending = false;
            _ackPort = null;

        }

        /// <summary>
        /// Internal Drive Power Handler
        /// Processes the most recent Drive Power command
        /// When complete, self activate for the next internal command
        /// </summary>
        /// <param name="driveDistance"></param>
        /// <returns></returns>
        public virtual IEnumerator<ITask> InternalDrivePowerHandler(drive.SetDrivePower power)
        {
            try
            {
                // Take a snapshot of the number of pending commands at the time
                // we entered this routine.
                // This will prevent a livelock which can occur if we try to
                // process the queue until it is empty, but the inbound queue
                // is growing at the same rate as we are pulling off the queue.
                int pendingCommands = _internalDrivePowerPort.ItemCount;

                // If newer commands have been issued, send success responses
                // to the older commands and move to the latest one
                drive.SetDrivePower newerUpdate;
                while (pendingCommands > 0)
                {
                    if (_internalDrivePowerPort.Test(out newerUpdate))
                    {
                        // Timed motion requests do not have a response initially,
                        // that comes later when the motors are stopped
                        if (power.ResponsePort != null)
                            power.ResponsePort.Post(DefaultUpdateResponseType.Instance);
                        power = newerUpdate;
                    }
                    pendingCommands--;
                }

                yield return Arbiter.Choice(_brickPort.SetDrivePower(new drive.SetDrivePowerRequest(power.Body.LeftWheelPower, power.Body.RightWheelPower)),
                    delegate(DefaultUpdateResponseType ok)
                    {
                        _state.LeftWheel.MotorState.CurrentPower = power.Body.LeftWheelPower;
                        _state.RightWheel.MotorState.CurrentPower = power.Body.RightWheelPower;
                        if (power.ResponsePort != null)
                            power.ResponsePort.Post(ok);
                    },
                    delegate(Fault fault)
                    {
                        if (power.ResponsePort != null)
                            power.ResponsePort.Post(fault);
                    }
                );

            }
            finally
            {
                // Wait one time for the next InternalDrivePower command
                Activate(Arbiter.ReceiveWithIterator(false, _internalDrivePowerPort, InternalDrivePowerHandler));
            }
            yield break;
        }

        #endregion

    }
}
